﻿using System;
using System.Collections.Generic;
using System.Linq;
using System.Text;

namespace Effects.Filters
{
    /// <summary>
    ///  Second order filter wit methods to create Butterworth filters.
    /// </summary>
    public class SecondOrderFilter : Filter
    {
        #region coefficients
        // normalized coefficients from transfer functions

        /// <summary>
        ///  Nominator coefficients.
        /// </summary>
        private double[] a;

        /// <summary>
        ///  Gets nominator coefficients from transfer function.
        /// </summary>
        public double[] A
        {
            get { return a; }
            set { a = value; }
        }

        /// <summary>
        ///  Denominator coefficients.
        /// </summary>
        private double[] b;

        /// <summary>
        ///  Gets denominator coefficients from transfer function.
        /// </summary>
        public double[] B
        {
            get { return b; }
            set { b = value; }
        }

        #endregion

        #region previous_inputs_and_outputs
        // previous Xs and Ys of the filter.

        /// <summary>
        ///  Previous inputs. 
        /// </summary>
        private double[] x;

        /// <summary>
        ///  Previous outputs.
        /// </summary>
        private double[] y;

        #endregion

        #region constructor
        // constructors 

        /// <summary>
        ///  Creates new 2nd order IIR filter with given normalized coefficients.
        /// </summary>
        /// <param name="a"> Normalized nominator ceofficients. </param>
        /// <param name="b"> Normalized denominator coefficients. </param>
        /// <remarks>
        ///  All coefficients are expected directly from filter's transfer function.
        /// </remarks>
        public SecondOrderFilter(double[] a, double[] b)
        {
            if (a.Length != 3)
                throw new ArgumentException("Array containing nominator coefficients has invalid length. For 2nd order filter expectd length is 3");

            if(b.Length != 3)
                throw new ArgumentException("Array containing denominator coefficients has invalid length. For 2nd order filter expectd length is 3");

            this.a = a;
            this.b = b;

            x = new double[3];
            y = new double[3];
        }

        /// <summary>
        ///  Creates new 2nd order IIR filter with given coefficients. If normalized argument is set to false it normalizes them itself.
        /// </summary>
        /// <param name="a"> Nominator coefficients. </param>
        /// <param name="b"> Denominator coefficients. </param>
        /// <param name="normalized"> Indicates whether given coefficients are normalized or not. </param>
        /// <remarks>
        ///  All coefficients are expected directly from filter's transfer function.
        /// </remarks>
        public SecondOrderFilter(double[] a, double[] b, bool normalized):this(a,b)
        {
            if (!normalized)
            {
                for (int i = 0; i < 3; i++)
                    this.a[i] /= this.b[0];

                for (int i = 1; i < 3; i++)
                    this.b[i] /= this.b[0];

                this.b[0] = 1;
            }


        }

        #endregion

        #region step_method
        // overridden step method

        /// <summary>
        ///  Filters input signal.
        /// </summary>
        /// <param name="x0"> Input signal </param>
        /// <returns> Filtered output signal </returns>
        public override double Step(double x0)
        {
            x[0] = x0;
            y[0] = 0;

            y[0] = a[0] * x[0] + a[1] * x[1] + a[2] * x[2] - b[1] * y[1] - b[2] * y[2];

            // move Xs and Ys
            x[2] = x[1];
            x[1] = x[0];

            y[2] = y[1];
            y[1] = y[0];

            return y[1];
        }

        #endregion

        #region creator_methods
        // method creates different types of 2nd order filters

        /// <summary>
        ///  Designs 2nd order Butterworth low-pass filter with given cut-off frequency and sampling rate.
        /// </summary>
        /// <param name="frequency"> Desired -3db cut-off frequency. </param>
        /// <param name="sampleRate"> Sampling rate of the input signal </param>
        /// <returns> SecondOrderFilter with appropriate coefficients </returns>
        public static SecondOrderFilter CreateLowPass(double frequency, int sampleRate)
        {
            // normalize frequncy
            double omega = frequency / sampleRate * 2 * Math.PI;

            // pre-warp the frequency
            double K = Math.Tan(omega / 2);
            double K2 = K * K;

            double[] a_test = new double[3];
            double[] b_test = new double[3];

            a_test[0] = K2;
            a_test[1] = 2 * K2;
            a_test[2] = K2;

            b_test[0] = 1 + Math.Sqrt(2) * K + K2;
            b_test[1] = 2 * (K2 - 1);
            b_test[2] = (K2 - Math.Sqrt(2) * K + 1);

            // ------------
            // normalized
            for (int i = 0; i < 3; i++)
                a_test[i] /= b_test[0];

            for (int i = 1; i < 3; i++)
                b_test[i] /= b_test[0];
            b_test[0] = 1;

            return new SecondOrderFilter(a_test, b_test);
        }

        /// <summary>
        ///  Designs 2nd order Butterworth high-pass filter with given cut-off frequency and sampling rate.
        /// </summary>
        /// <param name="frequency"> Desired -3db cut-off frequency. </param>
        /// <param name="sampleRate"> Sampling rate of the input signal </param>
        /// <returns> SecondOrderFilter with appropriate coefficients </returns>
        public static SecondOrderFilter CreateHighPass(double frequency, int sampleRate)
        {
            // normalize frequency
            double omega = (sampleRate / 2 - frequency) / sampleRate * 2 * Math.PI;

            // pre-warp frequency
            double K = Math.Tan(omega / 2);
            double K2 = K * K;

            double[] a_test = new double[3];
            double[] b_test = new double[3];

            a_test[0] = K2;
            a_test[1] = -2 * K2;
            a_test[2] = K2;

            b_test[0] = 1 + Math.Sqrt(2) * K + K2;
            b_test[1] = -2 * (K2 - 1);
            b_test[2] = (K2 - Math.Sqrt(2) * K + 1);

            // ------------
            // normalized
            for (int i = 0; i < 3; i++)
                a_test[i] /= b_test[0];

            for (int i = 1; i < 3; i++)
                b_test[i] /= b_test[0];
            b_test[0] = 1;

            return new SecondOrderFilter(a_test, b_test);
        }

        /// <summary>
        ///  Designs 2nd order Butterworth band-pass filter with given cut-off frequencies and sampling rate.
        /// </summary>
        /// <param name="freqLow"> Desired lower border frequency </param>
        /// <param name="freqHigh"> Desired upper border frequency </param>
        /// <param name="sampleRate"> Sampling rate of the input signal </param>
        /// <returns> SecondOrderFilter with appropriate coefficients </returns>
        public static SecondOrderFilter CreateBandPass(double freqLow, double freqHigh, int sampleRate)
        {
            // normalize frequencies
            double normalizedLow = freqLow / sampleRate * 2 * Math.PI;
            double normalizedHigh = freqHigh / sampleRate * 2 * Math.PI;

            // pre-warp frequencies
            double omegaLow = Math.Tan(normalizedLow / 2);
            double omegaHigh = Math.Tan(normalizedHigh / 2);

            double A = omegaHigh - omegaLow;
            double B = omegaLow * omegaHigh;

            double[] a = new double[3];
            double[] b = new double[3];

            a[0] = A;
            a[1] = 0;
            a[2] = -A;

            b[0] = 1 + A + B;
            b[1] = 2 * (B - 1);
            b[2] = 1 + B - A;

            return new SecondOrderFilter(a, b, false);
        }

        #endregion
    }
}
